package gnss

import (
	"dgo/goutils/utils"
	"fmt"
	"io"
	"math"
	"strings"
	"sync"
	"time"
)

/*
**

	2021-03-29 17:16:11
	// ProcObs, ProcSNR 加入处理锁
	// ObsV只处理一次, 避免其他多线程 读取数据时 内部数组被处理掉的情况

2021-12-20 14:22:16

	// 加入OnSignal 可以做信号的转换, 和芯星通 QZSS 8需要变成17
*/
type RtcmMsm struct {
	procLocker sync.RWMutex

	Sys     byte
	Freq    byte
	MsmTime time.Time
	Header  *RtcmMsmHeader

	startBitN int
	MsmType   int // msm4, msm5, msm7

	// 卫星数据
	RoughR [64]float64 //GNSS 卫星低精度星地距离（rough range）以 1 毫秒为除数得到的整数部分 DF397 +
	//GNSS 卫星低精度星地距离（rough range）以 1 毫秒为除数得到的1毫秒内的部分 DF398
	Ex      [64]byte
	PhaRate [64]float64

	// 信号数据
	PseRV []float64   // GNSS 信号高精度伪距（fine Psedoranges）  DF405
	PhaV  []float64   // GNSS 信号高精度载波相位(fine PhaseRange） DF406
	LokT  [64]uint16  // GNSS 载波相位锁定时间标识  DF407
	Half  [64]byte    // 半周模糊度标识  DF420
	Cnr   []float64   // GNSS 信号载噪比（CNRs）  DF408
	PhaRR [64]float64 // GNSS 信号高精度载波相位变化速率（fine PhaseRate） DF404

	ObsV []*Obs // 按照卫星索引进行存储

	Crc uint32

	RawBuf []byte // 整个MSM的原始BUF

	DecodeErr error

	OnSignal func(navisys byte, sigid *byte)
}

func NewRtcmMsm() *RtcmMsm {
	return &RtcmMsm{
		startBitN: 0,
		Header:    NewRtcmMsmHeader(),
	}
}

func (this *RtcmMsm) SetDecodeStartBitN(startBitN int) {
	this.startBitN = startBitN
}

/*
*
改变Signal
*/
func (this *RtcmMsm) ProcessSignal() {
	for i := U8_0; i < this.Header.SigN; i++ {
		sigv := this.Header.Sigs[i]
		if this.OnSignal != nil {
			this.OnSignal(this.Header.Sys, &sigv)
			this.Header.Sigs[i] = sigv
		}
	}
}

/*
**
  - 获取
    卫星的高精度伪距 (prng)
    信号高精度载波相位 (phrng)
    cnr
*/
func (this *RtcmMsm) GetObsV(satIdx byte) (prng []float64, phrng []float64, cnr []float64) {
	var i byte = 0
	var k byte = 0
	var j byte = 0
	if satIdx >= this.Header.SatN {
		return
	}
	prng = make([]float64, this.Header.SigN)
	phrng = make([]float64, this.Header.SigN)
	cnr = make([]float64, this.Header.SigN)
	for i = 0; i <= satIdx; i++ {
		for k = 0; k < this.Header.SigN; k++ {
			idx := k + i*this.Header.SigN
			if idx >= 64 {
				utils.Log.Errf("CellMask下标[%d]超过64, 卫星数(satN:%d), 信号数(SigN:%d)",
					idx, this.Header.SatN, this.Header.SigN)
				continue
			}
			m := this.Header.CellMask[idx]
			if m == 0 {
				continue
			}

			if i == satIdx {
				prng[k] = this.PseRV[j]
				phrng[k] = this.PhaV[j]
				cnr[k] = this.Cnr[j]
				//return;
			}

			j++
		}
	}

	return
}

func (this *RtcmMsm) String() string {
	if len(this.ObsV) == 0 {
		this.ProcObs()
	}
	var sb strings.Builder
	this.ToString(&sb)
	return sb.String()
}

/*
*

	统计SNR大于某个值的卫星
	statMap SYS->map(prn:flag)
*/
func (this *RtcmMsm) StatBySNR(statMap map[byte]map[byte]int, val float64) {
	prnMap := statMap[this.Sys]
	if prnMap == nil {
		prnMap = make(map[byte]int)
		statMap[this.Sys] = prnMap
	}

	var i, j byte

	// 循环卫星
	for i = 0; i < this.Header.SatN; i++ {
		prnId := this.Header.Sats[i]
		for j = 0; j < this.Header.SigN; j++ {
			if this.ObsV[i].Snr[j] >= val {
				prnMap[prnId] = 1
				break
			}
		}
	}

}

/*
*

	去重复

*
*/
func (this *RtcmMsm) CheckAddSign(sign []byte) []byte {
	var j byte = 0
	for j = 0; j < this.Header.SigN; j++ {
		v := this.Header.Sigs[j]
		if utils.SliceIndexByte(sign, v) == -1 {
			sign = append(sign, v)
		}
	}
	return sign
}

/*
**

	先要处理ProcV
	map[satid]map[sign]snr
	satid: -> map[sign]snr
	[1:[1:snr,2:snr],2:[sign:snr,sign:snr]]

*
*/
func (this *RtcmMsm) WriteSNR2Map(snrMap map[byte]map[byte]float64) {
	var i, j byte
	// 循环卫星
	for i = 0; i < this.Header.SatN; i++ {
		prnId := this.Header.Sats[i]
		prnMap := snrMap[prnId]
		if prnMap == nil {
			prnMap = make(map[byte]float64)
			snrMap[prnId] = prnMap
		}

		for j = 0; j < this.Header.SigN; j++ {
			idx := this.Header.Sigs[j]
			prnMap[idx] = this.ObsV[i].Snr[j]
		}
	}
}

func (this *RtcmMsm) WriteSimpleSNREx(sb *strings.Builder, sign []byte) {

	snrMap := make(map[byte]map[byte]float64)
	this.WriteSNR2Map(snrMap)

	// 循环卫星
	var i byte
	for i = 0; i < this.Header.SatN; i++ {
		prnId := this.Header.Sats[i]
		strPrn := GetPrnStrCode(prnId, this.Sys)
		sb.WriteString(strPrn)
		sb.WriteString("|")

		prnSNR := snrMap[prnId]
		if prnSNR == nil {
			continue
		}

		for j := 0; j < len(sign); j++ {
			if j > 0 {
				sb.WriteString(",")
			}
			sb.WriteString(fmt.Sprintf("%.2f", prnSNR[sign[j]]))
		}
		sb.WriteString(";")
	}
}

/*
*

	BeiDou;B1,B3,B2;C01|44.00,45.00,48.00;C02|42.00,44.00,47.00;C03|46.00,46.00,48.00;C04|40.00,43.00,45.00;C06|47.00,48.00,48.00;C09|46.00,45.00,47.00;C16|49.00,47.00,48.00;
*/
func (this *RtcmMsm) WriteSimpleSNR(sb *strings.Builder) {
	var i, j byte
	sb.WriteString(GetNavSysName(this.Sys))
	sb.WriteString(";")
	// 写入信号头
	for j = 0; j < this.Header.SigN; j++ {
		iSign := this.Header.Sigs[j]
		strBrandId := SigiBrandID(this.Sys, iSign)
		if j > 0 {
			sb.WriteString(",")
		}
		sb.WriteString(strBrandId)
	}
	sb.WriteString(";")

	// 循环卫星
	for i = 0; i < this.Header.SatN; i++ {
		prnId := this.Header.Sats[i]
		strPrn := GetPrnStrCode(prnId, this.Sys)
		sb.WriteString(strPrn)
		sb.WriteString("|")

		for j = 0; j < this.Header.SigN; j++ {
			if j > 0 {
				sb.WriteString(",")
			}
			sb.WriteString(fmt.Sprintf("%.2f", this.ObsV[i].Snr[j]))
		}
		sb.WriteString(";")
	}
}

func (this *RtcmMsm) WriteBrandSNR(sb *strings.Builder) {
	var i, j byte
	// 循环卫星
	for i = 0; i < this.Header.SatN; i++ {
		prnId := this.Header.Sats[i]
		strPrn := GetPrnStrCode(prnId, this.Sys)
		sb.WriteString(strPrn)
		sb.WriteString(" ")

		// 循环信号
		for j = 0; j < this.Header.SigN; j++ {
			iSign := this.Header.Sigs[j]
			strBrandId := SigiBrandID(this.Sys, iSign)
			sb.WriteString(fmt.Sprintf("%s %.2f ", strBrandId, this.ObsV[i].Snr[j]))
		}
		sb.WriteString("\n")
	}
}

func (this *RtcmMsm) ToBNCString(siteid string, sb io.Writer) {
	var i, j byte
	// 循环卫星
	for i = 0; i < this.Header.SatN; i++ {
		prnId := this.Header.Sats[i]
		strPrn := GetPrnStrCode(prnId, this.Sys)
		if len(siteid) > 0 {
			sb.Write([]byte(siteid))
			sb.Write([]byte(" "))
		}

		sb.Write([]byte(strPrn))
		sb.Write([]byte(" "))

		// 循环信号
		for j = 0; j < this.Header.SigN; j++ {
			iSign := this.Header.Sigs[j]
			sSign := SigiRinexCode(this.Sys, iSign)
			if len(sSign) == 0 {
				sSign = SigiRinexCode(this.Sys, iSign)
				sSign = fmt.Sprintf("%d", iSign)
			}
			v := this.ObsV[i]
			str := fmt.Sprintf("C%s %s L%s %s S%s %s D%s %s", sSign, RinexFormatFloat(v.PseV[j]),
				sSign, RinexFormatFloat(v.L[j]),
				sSign, RinexFormatFloat(v.Snr[j]),
				sSign, RinexFormatFloat(v.Doppler[j]),
			)
			sb.Write([]byte(str))
		}
		sb.Write([]byte("\n"))
	}
}

func (this *RtcmMsm) ToString(sb *strings.Builder) {
	var i, j byte
	// 循环卫星
	for i = 0; i < this.Header.SatN; i++ {
		prnId := this.Header.Sats[i]
		strPrn := GetPrnStrCode(prnId, this.Sys)
		sb.WriteString(strPrn)
		sb.WriteString(" ")

		// 循环信号
		for j = 0; j < this.Header.SigN; j++ {
			iSign := this.Header.Sigs[j]
			sSign := SigiRinexCode(this.Sys, iSign)
			if len(sSign) == 0 {
				sSign = SigiRinexCode(this.Sys, iSign)
				sSign = fmt.Sprintf("%d", iSign)
			}
			v := this.ObsV[i]
			str := fmt.Sprintf("C%s %s L%s %s S%s %s D%s %s", sSign, RinexFormatFloat(v.PseV[j]),
				sSign, RinexFormatFloat(v.L[j]),
				sSign, RinexFormatFloat(v.Snr[j]),
				sSign, RinexFormatFloat(v.Doppler[j]),
			)
			sb.WriteString(str)
		}
		sb.WriteString("\n")
	}
}

func (this *RtcmMsm) DetailString() string {
	var i, j byte
	var sb strings.Builder
	// 循环卫星
	for i = 0; i < this.Header.SatN; i++ {
		prnId := this.Header.Sats[i]
		strPrn := GetPrnStrCode(prnId, this.Sys)
		sb.WriteString(strPrn)
		sb.WriteString(" ")
		sb.WriteString(fmt.Sprintf("RhR %s", RinexFormatFloat(this.RoughR[i])))

		// 循环信号
		for j = 0; j < this.Header.SigN; j++ {
			iSign := this.Header.Sigs[j]
			sSign := SigiRinexCode(this.Sys, iSign)
			if len(sSign) == 0 {
				sSign = SigiRinexCode(this.Sys, iSign)
				sSign = fmt.Sprintf("%d", iSign)
			}
			v := this.ObsV[i]
			str := fmt.Sprintf("%s(%d): psrng:%s, phrng:%s, lkt:%d, halfamb:%d, L:%s, cnr:%s doppler:%s", sSign, iSign,
				RinexFormatFloat(v.PseV[j]),
				RinexFormatFloat(v.PhaV[j]),
				v.LTI[j],
				v.HalfC[j],
				RinexFormatFloat(v.L[j]),
				RinexFormatFloat(v.Snr[j]),
				RinexFormatFloat(v.Doppler[j]),
			)
			sb.WriteString(str)
		}
		sb.WriteString("\n")
	}
	return sb.String()
}

func (this *RtcmMsm) CheckSatIdsInvalid() error {
	var i byte = 0
	for i = 0; i < this.Header.SatN; i++ {
		iPrn := this.Header.Sats[i]
		err := CheckSatIdInvalid(iPrn, this.Sys)
		if err != nil {
			return err
		}
	}
	return nil
}

func (this *RtcmMsm) Proc4SNR(warn_func func(msg string)) {
	this.procLocker.Lock()
	defer this.procLocker.Unlock()

	if len(this.ObsV) > 0 {
		if warn_func != nil {
			warn_func("ObsV已经存在,请不要重复处理!")
		}
		return
	}

	this.ObsV = make([]*Obs, this.Header.SatN)
	var i byte = 0
	var k byte = 0
	var j byte = 0
	for i = 0; i < this.Header.SatN; i++ {
		obs := NewObs()
		iPrn := this.Header.Sats[i]
		obs.PrnID = iPrn
		obs.Snr = make([]float64, this.Header.SigN)

		this.ObsV[i] = obs // 第几颗卫星
		for k = 0; k < this.Header.SigN; k++ {
			idx := k + i*this.Header.SigN
			if idx >= 64 {
				if warn_func != nil {
					warn_func(fmt.Sprintf("CellMask下标[%d]超过64, 卫星数(satN:%d), 信号数(SigN:%d)\r\n",
						idx, this.Header.SatN, this.Header.SigN))
				}
				continue
			}
			m := this.Header.CellMask[idx]
			if m == 0 {
				continue
			}

			obs.Snr[k] = this.Cnr[j]
			j++
		}
	}

}

/***
 * 2021-03-29 17:12:59
 *    加入多线程操作处理
 */
func (this *RtcmMsm) ProcObs() (warnmsg string) {
	this.procLocker.Lock()
	defer this.procLocker.Unlock()

	if len(this.ObsV) > 0 {
		return "" // 已经处理, 不再处理
	}

	var sb utils.BytesBuilder
	this.ObsV = make([]*Obs, this.Header.SatN)
	var i byte = 0
	var k byte = 0
	var j byte = 0
	for i = 0; i < this.Header.SatN; i++ {
		obs := NewObs()
		iPrn := this.Header.Sats[i]
		obs.PrnID = iPrn
		obs.Prn = GetPrnStrCode(iPrn, this.Sys)
		obs.PseV = make([]float64, this.Header.SigN)
		obs.PhaV = make([]float64, this.Header.SigN)
		obs.L = make([]float64, this.Header.SigN)
		obs.Doppler = make([]float64, this.Header.SigN)
		obs.Snr = make([]float64, this.Header.SigN)
		obs.LTI = make([]uint16, this.Header.SigN)
		obs.HalfC = make([]byte, this.Header.SigN)

		this.ObsV[i] = obs // 第几颗卫星
		//var tmpP float64 = 0;
		for k = 0; k < this.Header.SigN; k++ {
			idx := k + i*this.Header.SigN
			if idx >= 64 {
				sb.Appendf("CellMask下标[%d]超过64, 卫星数(satN:%d), 信号数(SigN:%d)\r\n",
					idx, this.Header.SatN, this.Header.SigN)
				continue
			}
			m := this.Header.CellMask[idx]
			if m == 0 {
				continue
			}

			sigv := this.Header.Sigs[k]

			// 信号波长
			wl := SigWaveLen(this.Sys, sigv, iPrn)
			if wl <= 0.0 {
				sb.Appendf("[%d]%s, sign:%d 对应的 波长为0, 请进行核实, TOW:%d\r\n",
					this.Header.MsgId, obs.Prn, sigv,
					this.Header.TOW)
				continue
			}

			// pseudorange
			if this.RoughR[i] != 0.0 && this.PseRV[j] > -1e12 {
				if this.PseRV[j] == PS_INVALID_VALUE {
					obs.PseV[k] = 0
				} else {
					obs.PseV[k] = this.RoughR[i] + this.PseRV[j]
				}
			}
			// carrier-phase (cycle)
			if this.RoughR[i] != 0.0 && this.PhaV[j] > -1e12 && wl > 0 {
				if this.PhaV[j] == PH_INVALID_VALUE {
					obs.PseV[k] = 0
				} else {
					obs.L[k] = (this.RoughR[i] + this.PhaV[j]) / wl
				}
			}
			// doppler (hz)
			if this.PhaRR[i] > -1e12 && wl > 0 {
				obs.Doppler[k] = (this.PhaRate[i] + this.PhaRR[j]) / wl // rtklib
			}
			if CONFIG_OUT_DEBUG_RINEX_HEADER {
				fmt.Printf("[wl]%d, %f dop:%f(%f+%f)\n", iPrn, wl, obs.Doppler[k], this.PhaRate[i], this.PhaRR[j])
			}
			//fmt.Printf("[wl]%d, %f dop:%f(%f+%f)\n", iPrn, wl, obs.Doppler[k], this.PhaRate[i], this.PhaRR[j])
			obs.PhaV[k] = this.PhaV[j]
			obs.LTI[k] = this.LokT[j]
			obs.HalfC[k] = this.Half[j]

			//obs.Snr[k] = float64(uint8(this.Cnr[j] * 4.0)) / 4.0     // 0.25的整倍数
			obs.Snr[k] = this.Cnr[j]
			j++
		}
	}

	return sb.String()

}

func (this *RtcmMsm) SetSystem(sys byte) {
	this.Header.Sys = sys
	this.Sys = sys
}

// / encode rough range integer ms
func Encode_msm_int_rrng(outBuf []byte, startBitN int, rng [64]float64, n int) int {
	var i_ms uint = 0
	for j := 0; j < n; j++ {
		if rng[j] == 0 {
			i_ms = 255
		} else if rng[j] < 0.0 || rng[j] > RANGE_MS*255.0 {
			utils.Log.Warnf("msm rough range overflow rrng=%.3f", rng[j])
			i_ms = 255
		} else {
			i_ms = uint(RoundU(rng[j]/RANGE_MS/P2_10) >> 10)
		}
		SetBitU8(outBuf, startBitN, 8, uint8(i_ms))
		startBitN += 8
	}
	return startBitN
}

// 信号高精度伪距 (msm3, msm4, msm5)
func Encode_msm_psrng(outBuf []byte, startBitN int, psrng []float64, n int) int {
	var psrng_val int = 0
	for j := 0; j < n; j++ {
		if psrng[j] == 0 {
			psrng_val = -16384
		} else if math.Abs(psrng[j]) > 32767 {
			utils.Log.Warnf("msm fine pseudorange overflow psrng=%.3f", psrng[j])
			psrng_val = -16384
			//} else if math.Abs(psrng[j]) > 292.7 {
			//	utils.Log.Warnf("msm fine pseudorange overflow psrng=%.3f", psrng[j])
			//	psrng_val = -16384
		} else {
			v1 := psrng[j] / RANGE_MS / P2_24
			psrng_val = int(Round(v1))
		}
		SetBitI32(outBuf, startBitN, 15, int32(psrng_val))
		startBitN += 15
	}
	return startBitN
}

// 信号高精度伪距 (msm6, msm7)
func Encode_msm_psrng_ex(outBuf []byte, startBitN int, psrng []float64, n int) int {
	var psrng_val int = 0
	for j := 0; j < n; j++ {
		if psrng[j] == 0 {
			psrng_val = -524288
			//} else if math.Abs(psrng[j]) > 292.7 {
			//	utils.Log.Warnf("msm fine pseudorange overflow psrng=%.3f", psrng[j])
			//	psrng_val = -524288
			//	} else if math.Abs(psrng[j]) > 292.7 {
			//		utils.Log.Warnf("msm fine pseudorange overflow psrng=%.3f", psrng[j])
			//		psrng_val = -16384
		} else {
			v1 := psrng[j] / RANGE_MS / P2_29
			psrng_val = int(Round(v1))
		}
		SetBitI32(outBuf, startBitN, 20, int32(psrng_val))
		startBitN += 20
	}
	return startBitN
}

// 高精度载波
func Encode_msm_phrng(outBuf []byte, startBitN int, phrng []float64, n int) int {
	var val int = 0
	for j := 0; j < n; j++ {
		if phrng[j] == 0 {
			val = -2097152
		} else if math.Abs(phrng[j]) > 1171.0 {
			utils.Log.Warnf("msm fine phase-range overflow psrng=%.3f", phrng[j])
			val = -2097152
		} else {
			v1 := phrng[j] / RANGE_MS / P2_29
			val = int(Round(v1))
		}
		SetBitI32(outBuf, startBitN, 22, int32(val))
		startBitN += 22
	}
	return startBitN
}

// / 载波相位锁定时间标识
func Encode_msm_lock(outBuf []byte, startBitN int, vals [64]uint16, n int) int {

	for j := 0; j < n; j++ {
		SetBitU8(outBuf, startBitN, 4, uint8(vals[j]))
		startBitN += 4
	}
	return startBitN
}

// / 半周模糊度标识
func Encode_msm_half_amb(outBuf []byte, startBitN int, vals [64]byte, n int) int {
	for j := 0; j < n; j++ {
		SetBitU8(outBuf, startBitN, 1, uint8(vals[j]))
		startBitN += 1
	}
	return startBitN
}

// / GNSS 信号载噪比（CNRs）  DF408
func Encode_msm_cnr(outBuf []byte, startBitN int, vals []float64, n int) int {
	for j := 0; j < n; j++ {
		SetBitU8(outBuf, startBitN, 6, uint8(RoundU(vals[j])))
		startBitN += 6
	}
	return startBitN
}

/* encode rough range modulo 1 ms --------------------------------------------*/
func Encode_msm_mod_rrng(outBuf []byte, startBitN int, rng [64]float64, n int) int {
	var mod_ms uint = 0
	for j := 0; j < n; j++ {
		if rng[j] <= 0.0 || rng[j] > RANGE_MS*255.0 {
			utils.Log.Warnf("msm rough range overflow rrng=%.3f", rng[j])
			mod_ms = 0
		} else {
			mod_ms = uint(RoundU(rng[j]/RANGE_MS/P2_10) & 0x3FF)
		}
		SetBitU32(outBuf, startBitN, 10, uint32(mod_ms))
		startBitN += 10
	}
	return startBitN
}

func (this *RtcmMsm) EncodeMsm(outBuf []byte, startBitN int) (rval int) {
	if this.MsmType == 4 {
		return this.EncodeMsm4(outBuf, startBitN)
	} else {
		return -1
	}

}

func (this *RtcmMsm) ToRawMsm4(msm *RtcmRawMsm4) {
	var i_ms uint = 0
	var mod_ms uint = 0
	rng := this.RoughR
	n := this.Header.SatN
	//msm.RoughRng1 = make([]byte, n)
	//msm.RoughRng2 = make([]uint16, n)
	for j := U8_0; j < n; j++ {
		if rng[j] == 0 {
			i_ms = 255
			mod_ms = 0
		} else if rng[j] < 0.0 || rng[j] > RANGE_MS*255.0 {
			utils.Log.Warnf("msm rough range overflow rrng=%.3f", rng[j])
			i_ms = 255
			mod_ms = 0
		} else {
			ut1 := RoundU(rng[j] / RANGE_MS / P2_10)
			i_ms = uint(ut1 >> 10)
			mod_ms = uint(ut1 & 0x3FF)
		}
		msm.RoughRng1[j] = byte(i_ms)
		msm.RoughRng2[j] = uint16(mod_ms)
	}

	{
		n := int(this.Header.CellN)
		//msm.PsRng = make([]int16, n)
		//msm.PhRng = make([]int32, n)
		//msm.LkT = make([]byte, n)
		//msm.HalfAmb = make([]byte, n)
		//msm.CNR = make([]byte, n)
		var psrng_val int = 0
		for j := 0; j < n; j++ {
			{
				psrngv := this.PseRV[j]
				if psrngv == 0 {
					psrng_val = -16384
				} else if math.Abs(psrngv) > 32767 {
					utils.Log.Warnf("msm fine pseudorange overflow psrng=%.3f", psrngv)
					psrng_val = -16384
				} else {
					v1 := psrngv / RANGE_MS / P2_24
					psrng_val = int(Round(v1))
				}
				msm.PsRng[j] = int16(psrng_val)
			}

			{
				phrngv := this.PhaV[j]
				v := -2097152
				if math.Abs(phrngv) > 1171.0 {
					utils.Log.Warnf("msm fine phase-range overflow psrng=%.3f", phrngv)
					v = -2097152
				} else {
					v1 := phrngv / RANGE_MS / P2_29
					v = int(Round(v1))
				}
				msm.PhRng[j] = int32(v)
			}

			{
				msm.LkT[j] = byte(this.LokT[j])
				msm.HalfAmb[j] = this.Half[j]
				msm.CNR[j] = byte(RoundU(this.Cnr[j]))
			}
		}
	}

	//startBitN = Encode_msm_int_rrng(outBuf, startBitN, this.RoughR, int(this.Header.SatN))
	//startBitN = Encode_msm_mod_rrng(outBuf, startBitN, this.RoughR, int(this.Header.SatN))
	//startBitN = Encode_msm_psrng(outBuf, startBitN, this.PseRV, int(this.Header.CellN))
	//startBitN = Encode_msm_phrng(outBuf, startBitN, this.PhaV, int(this.Header.CellN))
	//startBitN = Encode_msm_lock(outBuf, startBitN, this.LokT, int(this.Header.CellN))
	//startBitN = Encode_msm_half_amb(outBuf, startBitN, this.Half, int(this.Header.CellN))
	//startBitN = Encode_msm_cnr(outBuf, startBitN, this.Cnr, int(this.Header.CellN))
	//return startBitN
}

func (this *RtcmMsm) EncodeMsm4(outBuf []byte, startBitN int) (rval int) {
	startBitN = this.Header.Encode(outBuf, startBitN)
	startBitN = Encode_msm_int_rrng(outBuf, startBitN, this.RoughR, int(this.Header.SatN))
	startBitN = Encode_msm_mod_rrng(outBuf, startBitN, this.RoughR, int(this.Header.SatN))
	startBitN = Encode_msm_psrng(outBuf, startBitN, this.PseRV, int(this.Header.CellN))
	startBitN = Encode_msm_phrng(outBuf, startBitN, this.PhaV, int(this.Header.CellN))
	startBitN = Encode_msm_lock(outBuf, startBitN, this.LokT, int(this.Header.CellN))
	startBitN = Encode_msm_half_amb(outBuf, startBitN, this.Half, int(this.Header.CellN))
	startBitN = Encode_msm_cnr(outBuf, startBitN, this.Cnr, int(this.Header.CellN))
	return startBitN
}

/**
 * 有错误, 待修正
 */
func (this *RtcmMsm) EncodeMsm7(outBuf []byte, startBitN int) (rval int) {
	startBitN = this.Header.Encode(outBuf, startBitN)
	startBitN = Encode_msm_int_rrng(outBuf, startBitN, this.RoughR, int(this.Header.SatN))
	startBitN = Encode_msm_mod_rrng(outBuf, startBitN, this.RoughR, int(this.Header.SatN))
	startBitN = Encode_msm_psrng_ex(outBuf, startBitN, this.PseRV, int(this.Header.CellN))
	startBitN = Encode_msm_phrng(outBuf, startBitN, this.PhaV, int(this.Header.CellN))
	startBitN = Encode_msm_lock(outBuf, startBitN, this.LokT, int(this.Header.CellN))
	startBitN = Encode_msm_half_amb(outBuf, startBitN, this.Half, int(this.Header.CellN))
	startBitN = Encode_msm_cnr(outBuf, startBitN, this.Cnr, int(this.Header.CellN))
	return startBitN
}

func (this *RtcmMsm) DecodeHeader(buf []byte) {
	this.Header.DecodeEx(buf, this.startBitN)
}

// 1074, 1124, 1084
func (this *RtcmMsm) DecodeMsm4(buf []byte) (rval int) {
	defer func() { // 捕捉异常
		if err := recover(); err != nil {
			utils.Log.Err("解码rtcm,msm出现了异常:", err)
			rval = -1
			return
		}
	}()

	bitsN, e1 := this.Header.DecodeEx(buf, this.startBitN)
	if e1 != nil {
		this.DecodeErr = e1
	}
	if bitsN <= this.startBitN {
		return -1
	}

	//if (this.Sys==SYS_BD){
	//	this.Header.TOW += 14 * 1000;
	//}

	// ========================================卫星数据
	for j := 0; j < int(this.Header.SatN); j++ {
		rng := GetBitU8(buf, bitsN, 8)
		bitsN += 8
		if rng != 255 {
			this.RoughR[j] = float64(rng) * RANGE_MS
		}
	}

	// GNSS 卫星低精度星地距离，以 1 毫秒为除数得到的1 毫秒内的部分
	for j := 0; j < int(this.Header.SatN); j++ {
		v := GetBitU(buf, bitsN, 10)
		bitsN += 10
		if this.RoughR[j] != 0.0 {
			this.RoughR[j] += float64(v) * P2_10 * RANGE_MS
		}
	}

	// ========================================信号数据
	// GNSS 信号高精度伪距（fine Psedoranges）   DF405
	this.PseRV = make([]float64, this.Header.CellN)
	this.PhaV = make([]float64, this.Header.CellN)
	this.Cnr = make([]float64, this.Header.CellN)

	for j := 0; j < int(this.Header.CellN); j++ { //  pseudorange
		v := GetBitI32(buf, bitsN, 15)
		bitsN += 15
		if v != -16384 {
			this.PseRV[j] = float64(v) * P2_24 * RANGE_MS
		} else {
			this.PseRV[j] = 0
		}
	}

	// GNSS 信号高精度载波相位（fine PhaseRange） DF406    phaserange
	for j := 0; j < int(this.Header.CellN); j++ {
		v := GetBitI32(buf, bitsN, 22)
		bitsN += 22
		if v != -2097152 {
			this.PhaV[j] = float64(v) * P2_29 * RANGE_MS
		}
	}

	// lock time
	for j := 0; j < int(this.Header.CellN); j++ {
		v := GetBitU8(buf, bitsN, 4)
		bitsN += 4
		this.LokT[j] = uint16(v)
	}

	// half-cycle ambiguity
	for j := 0; j < int(this.Header.CellN); j++ {
		v := GetBitU8(buf, bitsN, 1)
		bitsN += 1
		this.Half[j] = v
	}

	//
	for j := 0; j < int(this.Header.CellN); j++ {
		v := GetBitU8(buf, bitsN, 6)
		bitsN += 6
		this.Cnr[j] = float64(v)
	}

	return 0
}

// 1075, 1085
func (this *RtcmMsm) DecodeMsm5(buf []byte) (rval int) {
	defer func() { // 捕捉异常
		if err := recover(); err != nil {
			utils.Log.Err("解码rtcm,msm出现了异常:", err)
			rval = -1
			return
		}
	}()

	bitsN, e1 := this.Header.DecodeEx(buf, this.startBitN)
	if e1 != nil {
		this.DecodeErr = e1
	}
	if bitsN <= this.startBitN {
		return -1
	}

	//if (this.Sys==SYS_BD){
	//	this.Header.TOW += 14 * 1000;
	//}

	// ========================================卫星数据
	for j := 0; j < int(this.Header.SatN); j++ {
		rng := GetBitU8(buf, bitsN, 8)
		bitsN += 8
		if rng != 255 {
			this.RoughR[j] = float64(rng) * RANGE_MS
		}
	}

	for j := 0; j < int(this.Header.SatN); j++ {
		this.Ex[j] = GetBitU8(buf, bitsN, 4)
		bitsN += 4
	}

	// GNSS 卫星低精度星地距离，以 1 毫秒为除数得到的1 毫秒内的部分
	for j := 0; j < int(this.Header.SatN); j++ {
		v := GetBitU(buf, bitsN, 10)
		bitsN += 10
		if this.RoughR[j] != 0.0 {
			this.RoughR[j] += float64(v) * P2_10 * RANGE_MS
		}
	}

	// GNSS 卫星低精度的载波相位变化速率 phaserange rate
	for j := 0; j < int(this.Header.SatN); j++ {
		v := GetBitI32(buf, bitsN, 14)
		bitsN += 14
		if v != -8192 {
			this.PhaRate[j] = float64(v)
		}
	}

	// ========================================信号数据
	// GNSS 信号高精度伪距（fine Psedoranges）   DF405
	this.PseRV = make([]float64, this.Header.CellN)
	this.PhaV = make([]float64, this.Header.CellN)
	this.Cnr = make([]float64, this.Header.CellN)

	for j := 0; j < int(this.Header.CellN); j++ { //  pseudorange
		v := GetBitI32(buf, bitsN, 15)
		bitsN += 15
		if v != -16384 {
			this.PseRV[j] = float64(v) * P2_24 * RANGE_MS
		}
	}

	// GNSS 信号高精度载波相位（fine PhaseRange） DF406    phaserange
	for j := 0; j < int(this.Header.CellN); j++ {
		v := GetBitI32(buf, bitsN, 22)
		bitsN += 22
		if v != -2097152 {
			this.PhaV[j] = float64(v) * P2_29 * RANGE_MS
		}
	}

	// lock time
	for j := 0; j < int(this.Header.CellN); j++ {
		v := GetBitU8(buf, bitsN, 4)
		bitsN += 4
		this.LokT[j] = uint16(v)
	}

	// half-cycle ambiguity
	for j := 0; j < int(this.Header.CellN); j++ {
		v := GetBitU8(buf, bitsN, 1)
		bitsN += 1
		this.Half[j] = v
	}

	//
	for j := 0; j < int(this.Header.CellN); j++ {
		v := GetBitU8(buf, bitsN, 6)
		bitsN += 6
		this.Cnr[j] = float64(v)
	}

	for j := 0; j < int(this.Header.CellN); j++ { //  phaserangerate
		v := GetBitI32(buf, bitsN, 15)
		bitsN += 15
		if v != -16384 {
			this.PhaRR[j] = float64(v) * 0.0001
		}
	}

	return 0
}

/*
*

	Extended L1&L2 GPS RTK Observables
*/
func (this *RtcmMsm) DecodeRTK1004(buf []byte) (rval int) {
	defer func() { // 捕捉异常
		if err := recover(); err != nil {
			utils.Log.Err("解码rtcm,msm出现了异常:", err)
			rval = -1
			return
		}
	}()

	bitsN, e1 := this.Header.DecodeEx(buf, this.startBitN)
	if e1 != nil {
		this.DecodeErr = e1
	}
	if bitsN <= this.startBitN {
		return -1
	}

	bitsN = this.startBitN + 64

	var sys byte = 0
	n := 0
	for j := 0; j < int(this.Header.SatN) && n < MAX_OBS_N && bitsN+125 <= len(buf)*8; j++ {
		prn := GetBitU(buf, bitsN, 6)
		bitsN += 6
		code1 := GetBitU(buf, bitsN, 1)
		bitsN += 1
		pr1 := GetBitU(buf, bitsN, 24)
		bitsN += 24
		ppr1 := GetBitI32(buf, bitsN, 20)
		bitsN += 20
		lock1 := GetBitU(buf, bitsN, 7)
		bitsN += 7
		amb := GetBitU(buf, bitsN, 8)
		bitsN += 8
		cnr1 := GetBitU(buf, bitsN, 8)
		bitsN += 8

		code2 := GetBitU(buf, bitsN, 2)
		bitsN += 2
		pr2 := GetBitU(buf, bitsN, 14)
		bitsN += 14
		ppr2 := GetBitI32(buf, bitsN, 20)
		bitsN += 20
		lock2 := GetBitU(buf, bitsN, 7)
		bitsN += 7
		cnr2 := GetBitU(buf, bitsN, 8)
		bitsN += 8

		sys = SYS_GPS
		//if prn < 40{
		//	sys = SYS_GPS
		//}else{
		//	sys = SYS_SBS
		//	prn += 80;
		//}

		{
			err := CheckSatIdInvalid(byte(prn), sys)
			if err != nil {
				utils.Log.WarnTypef("GNSS", "错误的卫星号, sys:%d, prn:%d", sys, prn)
				continue
			}
		}

		fmt.Println("DecodeRTK1004", prn, code1, pr1, ppr1, lock1, amb, cnr1, "|", code2, pr2, ppr2, lock2, cnr2)
		obs := NewObs()
		obs.Prn = GetPrnStrCode(byte(prn), sys)
		obs.PrnID = byte(prn)
		obs.PseV = make([]float64, 2)
		obs.L = make([]float64, 2)
		obs.LTI = make([]uint16, 2)
		//obs.PhaV
		pr1_f := float64(pr1)*0.02 + float64(amb)*PRUNIT_GPS
		if uint32(ppr1) != uint32(0xFFF80000) {
			obs.PseV[0] = pr1_f // 伪距
			cp1 := pr1_f * 0.0005 / (CIGHT_SPEED / FREQ_GPS_L1)
			obs.L[0] = cp1
		}

		n++
	}
	return 0
}

// msm7, 1127, 1077, 1087, 1097
func (this *RtcmMsm) DecodeMsm7(buf []byte) (rval int) {
	defer func() { // 捕捉异常
		if err := recover(); err != nil {
			this.DecodeErr = fmt.Errorf("解码rtcm,msm出现了异常:%v", err)
			utils.Log.Err("解码rtcm,msm出现了异常:", err)
			rval = -1
			return
		}
	}()

	bitsN, e1 := this.Header.DecodeEx(buf, this.startBitN)
	if e1 != nil {
		this.DecodeErr = e1
	}
	if bitsN <= this.startBitN {
		return -1
	}

	// ========================================卫星数据
	for j := 0; j < int(this.Header.SatN); j++ {
		rng := GetBitU8(buf, bitsN, 8)
		bitsN += 8
		if rng != 255 {
			this.RoughR[j] = float64(rng) * RANGE_MS
		}
	}

	for j := 0; j < int(this.Header.SatN); j++ {
		this.Ex[j] = GetBitU8(buf, bitsN, 4)
		bitsN += 4
	}

	// GNSS 卫星低精度星地距离，以 1 毫秒为除数得到的1 毫秒内的部分
	for j := 0; j < int(this.Header.SatN); j++ {
		v := GetBitU(buf, bitsN, 10)
		bitsN += 10
		if this.RoughR[j] != 0.0 {
			this.RoughR[j] += float64(v) * P2_10 * RANGE_MS
		}
	}

	// GNSS 卫星低精度的载波相位变化速率 phaserange rate
	for j := 0; j < int(this.Header.SatN); j++ {
		v := GetBitI32(buf, bitsN, 14)
		bitsN += 14
		if v != -8192 {
			this.PhaRate[j] = float64(v)
		}
	}

	// ========================================信号数据
	// GNSS 信号高精度伪距（fine Psedoranges）   DF405
	this.PseRV = make([]float64, this.Header.CellN)
	this.PhaV = make([]float64, this.Header.CellN)
	this.Cnr = make([]float64, this.Header.CellN)

	for j := 0; j < int(this.Header.CellN); j++ {
		v := GetBitI32(buf, bitsN, 20)

		bitsN += 20
		if v != -524288 {
			this.PseRV[j] = float64(v) * P2_29 * RANGE_MS
			//logger.Debug("BIT32:", buf, bitsN, v, this.PseV[j]);
		} else {
			this.PseRV[j] = 0
		}
	}

	// GNSS 信号高精度载波相位（fine PhaseRange） DF406
	for j := 0; j < int(this.Header.CellN); j++ {
		v := GetBitI32(buf, bitsN, 24)
		bitsN += 24
		if v != -8388608 {
			this.PhaV[j] = float64(v) * P2_31 * RANGE_MS
		} else {
			this.PhaV[j] = 0
		}
	}

	// GNSS 载波相位锁定时间标识 DF407
	for j := 0; j < int(this.Header.CellN); j++ {
		v := GetBitU16(buf, bitsN, 10)
		bitsN += 10
		this.LokT[j] = v
	}

	// HalfC
	for j := 0; j < int(this.Header.CellN); j++ {
		v := GetBitU8(buf, bitsN, 1)
		bitsN += 1
		this.Half[j] = v
	}

	for j := 0; j < int(this.Header.CellN); j++ {
		v := GetBitU16(buf, bitsN, 10)
		bitsN += 10
		this.Cnr[j] = float64(v) * 0.0625
	}

	for j := 0; j < int(this.Header.CellN); j++ {
		v := GetBitI32(buf, bitsN, 15)
		bitsN += 15
		if v != -16384 {
			this.PhaRR[j] = float64(v) * 0.0001
		}
	}
	return bitsN
}
